// Copyright 2020 ros2_control Development Team
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include "cranebot_hardware/cranebot_system_position_only.hpp"
#include <chrono>
#include <cmath>
#include <limits>
#include <memory>
#include <vector>
#include <map>

#include "hardware_interface/types/hardware_interface_type_values.hpp"
#include "rclcpp/rclcpp.hpp"
//switcher_->switch_to(std::map<int, int>{{21,1},{22,1},{23,1},{27,1},{24,1},{28,1},{25,0},{29,0}});



namespace cranebot_hardware
{
hardware_interface::return_type CranebotSystemPositionOnlyHardware::configure(
  const hardware_interface::HardwareInfo & info)
{
  if (configure_default(info) != hardware_interface::return_type::OK)
  {
    return hardware_interface::return_type::ERROR;
  }

  hw_start_sec_ = stod(info_.hardware_parameters["example_param_hw_start_duration_sec"]);
  hw_stop_sec_ = stod(info_.hardware_parameters["example_param_hw_stop_duration_sec"]);
  hw_slowdown_ = stod(info_.hardware_parameters["example_param_hw_slowdown"]);
  hw_states_.resize(info_.joints.size(), std::numeric_limits<double>::quiet_NaN());
  hw_commands_.resize(info_.joints.size(), std::numeric_limits<double>::quiet_NaN());

  for (const hardware_interface::ComponentInfo & joint : info_.joints)
  {
    // cranebotSystemPositionOnly has exactly one state and command interface on each joint
    if (joint.command_interfaces.size() != 1)
    {
      RCLCPP_FATAL(
        rclcpp::get_logger("CranebotSystemPositionOnlyHardware"),
        "Joint '%s' has %d command interfaces found. 1 expected.", joint.name.c_str(),
        joint.command_interfaces.size());
      return hardware_interface::return_type::ERROR;
    }

    if (joint.command_interfaces[0].name != hardware_interface::HW_IF_POSITION)
    {
      RCLCPP_FATAL(
        rclcpp::get_logger("CranebotSystemPositionOnlyHardware"),
        "Joint '%s' have %s command interfaces found. '%s' expected.", joint.name.c_str(),
        joint.command_interfaces[0].name.c_str(), hardware_interface::HW_IF_POSITION);
      return hardware_interface::return_type::ERROR;
    }

    if (joint.state_interfaces.size() != 1)
    {
      RCLCPP_FATAL(
        rclcpp::get_logger("CranebotSystemPositionOnlyHardware"),
        "Joint '%s' has %d state interface. 1 expected.", joint.name.c_str(),
        joint.state_interfaces.size());
      return hardware_interface::return_type::ERROR;
    }  RCLCPP_INFO(rclcpp::get_logger("CranebotSystemPositionOnlyHardware"), "Joints successfully read!");

    if (joint.state_interfaces[0].name != hardware_interface::HW_IF_POSITION)
    {
      RCLCPP_FATAL(
        rclcpp::get_logger("CranebotSystemPositionOnlyHardware"),
        "Joint '%s' have %s state interface. '%s' expected.", joint.name.c_str(),
        joint.state_interfaces[0].name.c_str(), hardware_interface::HW_IF_POSITION);
      return hardware_interface::return_type::ERROR;
    }
  }

  status_ = hardware_interface::status::CONFIGURED;
  return hardware_interface::return_type::OK;
}

std::vector<hardware_interface::StateInterface>
CranebotSystemPositionOnlyHardware::export_state_interfaces()
{
  std::vector<hardware_interface::StateInterface> state_interfaces;
  for (uint i = 0; i < info_.joints.size(); i++)
  {
    state_interfaces.emplace_back(hardware_interface::StateInterface(
      info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_states_[i]));
  }

  return state_interfaces;
}

std::vector<hardware_interface::CommandInterface>
CranebotSystemPositionOnlyHardware::export_command_interfaces()
{
  std::vector<hardware_interface::CommandInterface> command_interfaces;
  for (uint i = 0; i < info_.joints.size(); i++)
  {
    command_interfaces.emplace_back(hardware_interface::CommandInterface(
      info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_commands_[i]));
  }

  return command_interfaces;
}

hardware_interface::return_type CranebotSystemPositionOnlyHardware::start()
{
  RCLCPP_INFO(rclcpp::get_logger("CranebotSystemPositionOnlyHardware"), "Starting ...please wait...");
  sensor_.reset(new WT61C());
  pull_encoder_.reset(new PullEncoder());
  motor_encoder_.reset(new RotaryEncoder());
  sensor_->update();
  init_angle_[0] = sensor_->Angle[0]; init_angle_[1] = sensor_->Angle[1]; init_angle_[2] = sensor_->Angle[2];
  switcher_.reset(new GpioSwitcher(std::vector<int>{21, 22, 23, 24, 25, 27, 28, 29}));
  switcher_->switch_to(std::map<int, int>{{21,1},{22,1},{23,1},{27,1},{24,1},{28,1},{25,0},{29,0}});
  for (int i = 0; i < hw_start_sec_; i++)
  {
    rclcpp::sleep_for(std::chrono::seconds(1));
    RCLCPP_INFO(
      rclcpp::get_logger("CranebotSystemPositionOnlyHardware"), "%.1f seconds left...",
      hw_start_sec_ - i);
  }

  // set some default values when starting the first time
  for (uint i = 0; i < hw_states_.size(); i++)
  {
    if (std::isnan(hw_states_[i]))
    {
      hw_states_[i] = 0;
      hw_commands_[i] = 0;
      hw_pre_commands_.push_back(hw_commands_[i]);
      hw_new_commands_flag_.push_back(false);
    }
    else
    {
      hw_commands_[i] = hw_states_[i];
      hw_pre_commands_.push_back(hw_commands_[i]);
      hw_new_commands_flag_.push_back(false);
    }
  }

  status_ = hardware_interface::status::STARTED;

  RCLCPP_INFO(
    rclcpp::get_logger("CranebotSystemPositionOnlyHardware"), "System Successfully started!");

  return hardware_interface::return_type::OK;
}

hardware_interface::return_type CranebotSystemPositionOnlyHardware::stop()
{
  RCLCPP_INFO(rclcpp::get_logger("CranebotSystemPositionOnlyHardware"), "Stopping ...please wait...");

  for (int i = 0; i < hw_stop_sec_; i++)
  {
    rclcpp::sleep_for(std::chrono::seconds(1));
    RCLCPP_INFO(
      rclcpp::get_logger("CranebotSystemPositionOnlyHardware"), "%.1f seconds left...",
      hw_stop_sec_ - i);
  }

  status_ = hardware_interface::status::STOPPED;

  RCLCPP_INFO(
    rclcpp::get_logger("CranebotSystemPositionOnlyHardware"), "System successfully stopped!");
    
  return hardware_interface::return_type::OK;
}

hardware_interface::return_type CranebotSystemPositionOnlyHardware::read()
{
  //RCLCPP_INFO(rclcpp::get_logger("CranebotSystemPositionOnlyHardware"), "Reading...");

  sensor_->update();
  //RCLCPP_INFO(rclcpp::get_logger("CranebotSystemPositionOnlyHardware"), "rotate angle: %f raise angle: %f", hw_states_[0], hw_states_[1]);
  for (uint i = 0; i < hw_states_.size(); i++)
  {
    switch (i)
    {
    case 0: //rotate
      /* code: read from the IMU (z) */
      hw_states_[i] = (sensor_->Angle[2]-init_angle_[2])/180.0*3.1416;
      break;

    case 1: //raise
      /* code: read from the IMU (y) */
      hw_states_[i] = -(sensor_->Angle[1]-init_angle_[1])/180.0*3.1416;
      break;

    case 2: //extend
      /* code: read from the encoder(pull string) */
      hw_states_[i] = pull_encoder_->get_pos() / 100.0 * 0.08;
      break;

    case 3: //pulley
      hw_states_[i] = -hw_states_[1];
      break;

    case 4: //lift
      /* code: read from the encoder(motor encoder) */
      hw_states_[i] = ((motor_encoder_->get_pos()/2860.0*(0.0545*3.1416)) - hw_states_[2]) / 4.0;
      break;
          
    default:
      break;
    }
  }
  return hardware_interface::return_type::OK;
}

hardware_interface::return_type CranebotSystemPositionOnlyHardware::write()
{
  //RCLCPP_INFO(rclcpp::get_logger("CranebotSystemPositionOnlyHardware"), "Writing...");

  for (uint i = 0; i < hw_commands_.size(); i++)
  {
    //This is used to judge if the switch should be opened.
    //if the command is the same as the previous command, the command changed flag remain false.
    if (hw_commands_[i] != hw_pre_commands_[i])
    {
      hw_pre_commands_[i] = hw_commands_[i]; 
      RCLCPP_INFO(rclcpp::get_logger("CranebotSystemPositionOnlyHardware"), "Get new commands %.5f for '%s'!", hw_commands_[i], info_.joints[i].name.c_str());
      hw_new_commands_flag_[i] = true;
      //if the command is larger than the current states, do something regard to the joint type.
      if (hw_commands_[i] > hw_states_[i])
      {
        //TODO: cranebot_switch.set_joint(joint[i], POS)
        switch (i)
        {
        case 0: //rotate
          switcher_->switch_to(std::map<int, int>{{ROTATE_LEFT, 0}, {ROTATE_RIGHT, 1}});
          break;

        case 1: //raise
          switcher_->switch_to(std::map<int, int>{{RAISE_ENABLE, 0}, {RAISE_FORWARD, 0}});
          break;

        case 2: //extend
          switcher_->switch_to(std::map<int, int>{{EXTEND_ENABLE, 0}, {EXTEND_FORWARD, 0}});
          break;

        case 3: //pulley
          break;

        case 4: //lift
          switcher_->switch_to(std::map<int, int>{{LIFT_UP, 0}, {LIFT_DOWN, 1}});
          break;
        }
      }
      else
      {
        switch (i)
        {
        case 0: //rotate
          switcher_->switch_to(std::map<int, int>{{ROTATE_LEFT, 1}, {ROTATE_RIGHT, 0}});
          break;

        case 1: //raise
          switcher_->switch_to(std::map<int, int>{{RAISE_ENABLE, 0}, {RAISE_FORWARD, 1}});
          break;

        case 2: //extend
          switcher_->switch_to(std::map<int, int>{{EXTEND_ENABLE, 0}, {EXTEND_FORWARD, 1}});
          break;

        case 3: //pulley
          break;

        case 4: //lift
          switcher_->switch_to(std::map<int, int>{{LIFT_UP, 1}, {LIFT_DOWN, 0}});
          break;
        }
      }
    }
    else
    {
      //do nothing
    }
  }

  //This is used to judge if the switch should be closed.
  for (uint i = 0; i < hw_states_.size(); i++)
  {
    //if commands changed
    if (hw_new_commands_flag_[i])
    {
      //if states is closed enough to the commands, shut down the switch. Open until the commands is changed.
      if (fabs(hw_states_[i]-hw_commands_[i])<0.01)
      {
        RCLCPP_INFO(rclcpp::get_logger("CranebotSystemPositionOnlyHardware"), "Commands %.5f for '%s' has been reached!", hw_commands_[i], info_.joints[i].name.c_str());
        switch (i)
        {
        case 0: //rotate
          switcher_->switch_to(std::map<int, int>{{ROTATE_LEFT, 1}, {ROTATE_RIGHT, 1}});
          break;

        case 1: //raise
          switcher_->switch_to(std::map<int, int>{{RAISE_ENABLE, 1}, {RAISE_FORWARD, 1}});
          break;

        case 2: //extend
          switcher_->switch_to(std::map<int, int>{{EXTEND_ENABLE, 1}, {EXTEND_FORWARD, 1}});
          break;

        case 3: //pulley
          hw_states_[i] = -hw_states_[1];
          break;

        case 4: //lift
          switcher_->switch_to(std::map<int, int>{{LIFT_UP, 0}, {LIFT_DOWN, 0}});
          break;
        }
        hw_new_commands_flag_[i] = false;
      }
      else{
        //do nothing
      }
    }
    else
    {
      //do nothing
    }
  }


  return hardware_interface::return_type::OK;
}

}  // namespace cranebot_hardware

#include "pluginlib/class_list_macros.hpp"

PLUGINLIB_EXPORT_CLASS(
  cranebot_hardware::CranebotSystemPositionOnlyHardware, hardware_interface::SystemInterface)
